/**
 * @FilePath     : /src/lgmg_robots/agv_navigation/agv_navigate/include/agvNavigate/helpInc/plan_target_info.h
 * @Description  : 定义了PlanTargetInfo类，用于管理和更新规划目标点的信息，包括轨迹类型、起点、终点和终点速度。 
 *                 还包含抑制分割的功能，能够判断是否需要规划和更新目标信息。 
 * @Author       : xiujun.yang
 * @Version      : 1.0.0
 * @LastEditors  : haibo haibo.yang@lgmgim.cn
 * @LastEditTime : 2024-12-19 16:52:26
 * @Copyright (c) 2024 by 临工智能信息科技有限公司, All Rights Reserved. 
**/
#pragma once  
#include "rclcpp/rclcpp.hpp"
#include "tools.h"
#include "trajectory.h"
#include "offline_scurve_plan.h"
#include <atomic>
// 规划目标点信息
class PlanTargetInfo
{
public:
    // 规划目标点结束时信息
    struct DataType {
        short traj_type;   // 目标结束路段的轨迹类型
        USHORT start_node; // 起点
        USHORT end_node;   // 终点
        float end_value;   // 终点速度

        DataType() {
            traj_type = static_cast<short>(TrajType::UNDEFINED_TRAJ);
            start_node = end_node = 65535;
            end_value = 0;
        }

        void Update(short type, USHORT start, USHORT end, float end_vel) {
            traj_type = type;
            start_node = start;
            end_node = end;
            end_value = end_vel;
            DEBUG_OUT("DataType update: type=" << type << ", start=" << start << ", end=" << end << ", endVel=" << end_vel);
        }
    };

    struct RefrainDivide {
        std::atomic<bool> flag; // 抑制分割标识
        float vel;

        RefrainDivide() {
            Clear();
        }

        void Clear() {
            flag = false;
            vel = 0;
        }
    };

    PlanTargetInfo() {
        SetPlanFlag(true);
        Clear();
    }

    // 清除记录的信息
    void Clear() {
        info_ = DataType();
        refrain_.Clear();
        DEBUG_OUT("PlanTargetInfo, clear all traj info once, and clear refrainDivide");
    }

    // 设置是否规划标识
    void SetPlanFlag(bool flag) {
        need_ = flag;
        DEBUG_OUT("PlanTargetInfo, set needPlan: " << need_);
    }

    // 是否需要规划
    bool NeedPlan() const {
        return need_;
    }

    // 更新数据
    void Update(short type, USHORT start, USHORT end, float end_vel) {
        info_.Update(type, start, end, end_vel);
        DEBUG_OUT("PlanTargetInfo update: type=" << type << ", start=" << start << ", end=" << end << ", endVel=" << end_vel);
    }


    bool PlanFinished(short cur_type, USHORT cur_start, USHORT cur_end) const {
        bool ret = info_.traj_type == cur_type && info_.start_node == cur_start && info_.end_node == cur_end;
        DEBUG_OUT("PlanTargetInfo planFinished=" << ret
                                                  << ", target: type=" << info_.traj_type
                                                  << ", start=" << info_.start_node
                                                  << ", end=" << info_.end_node);
        return ret;
    }

    // 获取当前规划目标信息
    bool GetInfo(DataType& info) const {
        info = info_;
        bool valid = info_.traj_type != static_cast<short>(TrajType::UNDEFINED_TRAJ) && 
                     info_.start_node != 65535 && 
                     info_.end_node != 65535;
        return valid;
    }

    // 信息是否相同
    bool Same(const DataType& other_info) const {
        return info_.traj_type == other_info.traj_type && 
               info_.start_node == other_info.start_node && 
               info_.end_node == other_info.end_node;
    }

    // 改变停车点
    bool StopPointChange(const DataType& other_info) {
        if (Same(other_info)) {
            // 原来规划需要停车
            bool stop_old = std::fabs(info_.end_value) < 0.001f;
            if (!stop_old) {
                return false;
            }
            // 修改过终点停车速度
            bool stop_new = std::fabs(other_info.end_value) < 0.001f;
            if (!stop_new) {
                DEBUG_OUT("stopPointChange OK;"
                          << ", old: target: type=" << info_.traj_type
                          << ", start=" << info_.start_node
                          << ", end=" << info_.end_node
                          << ", endV=" << info_.end_value
                          << ", new: type=" << other_info.traj_type
                          << ", start=" << other_info.start_node
                          << ", end=" << other_info.end_node
                          << ", endV=" << other_info.end_value);
                return true;
            }
        }
        return false;
    }

    // 抑制分割消息
    void SetRefrain(bool flag, float vel) {
        refrain_.flag = flag;
        refrain_.vel = vel;
        DEBUG_OUT("SetRefrain: flag=" << flag << ", vel=" << vel);
    }

    // 检测是否分割
    bool CheckRefrain(float cur_v) const {
        if (refrain_.flag) {
            bool v_match = cur_v <= refrain_.vel + 0.0000001f;
            DEBUG_OUT("CheckRefrain: "
                      << ", vMatch=" << v_match
                      << ", curV=" << cur_v
                      << ", refrain.vel=" << refrain_.vel);
            return v_match;
        }
        return false;
    }

    DataType info_;
    RefrainDivide refrain_; // 抑制信息记录
    std::atomic<bool> need_;
};
